#include "Car.h"

// 构造函数，初始化四个车轮
Car::Car() :
  L_T_Wheel("左上车轮", WHEEL_L_T_FORWARD_PIN, WHEEL_L_T_REVERSE_PIN),
  L_B_Wheel("左下车轮", WHEEL_L_B_FORWARD_PIN, WHEEL_L_B_REVERSE_PIN),
  R_T_Wheel("右上车轮", WHEEL_R_T_FORWARD_PIN, WHEEL_R_T_REVERSE_PIN),
  R_B_Wheel("右下车轮", WHEEL_R_B_FORWARD_PIN, WHEEL_R_B_REVERSE_PIN)
{}

// 启动小车
OperateStatus Car::startCar(bool leftRotation, bool leftSlide, bool rightRotation, bool rightSlide, int radius, int x, int y) {
    int maxDuty = 700;  // pwm最大调速值
    int xDuty = map(abs(x), 0, radius, 0, maxDuty);
    int yDuty = map(abs(y), 0, radius, 0, maxDuty);
    int diagonalXY = sqrt(pow(abs(x), 2) + pow(abs(y), 2));
    diagonalXY = diagonalXY > radius ? radius : diagonalXY;
    int diagonalXYDuty = map(diagonalXY, 0, radius, 0, maxDuty);

    // 左旋转
    if(leftRotation == true) {
        int duty = diagonalXYDuty < 10 ? maxDuty : diagonalXYDuty;
        L_T_Wheel.operateMotor(REVERSE, duty);
        L_B_Wheel.operateMotor(REVERSE, duty);
        R_T_Wheel.operateMotor(FORWARD, duty);
        R_B_Wheel.operateMotor(FORWARD, duty);
        return SUCCESS;
    } 

    // 右旋转
    if(rightRotation == true) {
        int duty = diagonalXYDuty < 10 ? maxDuty : diagonalXYDuty;
        L_T_Wheel.operateMotor(FORWARD, duty);
        L_B_Wheel.operateMotor(FORWARD, duty);
        R_T_Wheel.operateMotor(REVERSE, duty);
        R_B_Wheel.operateMotor(REVERSE, duty);      
        return SUCCESS; 
    } 

    // 左平移
    if (leftSlide == true) {
        int duty = diagonalXYDuty < 10 ? maxDuty : diagonalXYDuty;
        L_T_Wheel.operateMotor(REVERSE, duty);  
        L_B_Wheel.operateMotor(FORWARD, duty); 
        R_T_Wheel.operateMotor(FORWARD, duty); 
        R_B_Wheel.operateMotor(REVERSE, duty); 
        return SUCCESS;
    } 
    // 右平移
    else if (rightSlide == true) {
        int duty = diagonalXYDuty < 10 ? maxDuty : diagonalXYDuty;
        L_T_Wheel.operateMotor(FORWARD, duty); 
        L_B_Wheel.operateMotor(REVERSE, duty); 
        R_T_Wheel.operateMotor(REVERSE, duty); 
        R_B_Wheel.operateMotor(FORWARD, duty); 
        return SUCCESS;
    } 

    // 停止
    if (y == 0 && x == 0) {
        L_T_Wheel.operateMotor(STOP, 0);
        L_B_Wheel.operateMotor(STOP, 0);
        R_T_Wheel.operateMotor(STOP, 0);
        R_B_Wheel.operateMotor(STOP, 0);
        return SUCCESS;
    }  

    yDuty -= 5;
    diagonalXYDuty += 5;

    // 前进
    if (y > 0) {
        if (x == 0) {  
            L_T_Wheel.operateMotor(FORWARD, diagonalXYDuty); 
            L_B_Wheel.operateMotor(FORWARD, diagonalXYDuty); 
            R_T_Wheel.operateMotor(FORWARD, diagonalXYDuty); 
            R_B_Wheel.operateMotor(FORWARD, diagonalXYDuty); 
            return SUCCESS;
        } else if (x < 0) { // 左转
            L_T_Wheel.operateMotor(FORWARD, yDuty); 
            L_B_Wheel.operateMotor(FORWARD, yDuty); 
            R_T_Wheel.operateMotor(FORWARD, diagonalXYDuty); 
            R_B_Wheel.operateMotor(FORWARD, diagonalXYDuty);
            return SUCCESS;
        } else if (x > 0) { // 右转
            L_T_Wheel.operateMotor(FORWARD, diagonalXYDuty); 
            L_B_Wheel.operateMotor(FORWARD, diagonalXYDuty); 
            R_T_Wheel.operateMotor(FORWARD, yDuty); 
            R_B_Wheel.operateMotor(FORWARD, yDuty); 
            return SUCCESS;
        } 
    }
    // 后退
    else if (y < 0) {
        if (x == 0) {  
            L_T_Wheel.operateMotor(REVERSE, diagonalXYDuty); 
            L_B_Wheel.operateMotor(REVERSE, diagonalXYDuty); 
            R_T_Wheel.operateMotor(REVERSE, diagonalXYDuty); 
            R_B_Wheel.operateMotor(REVERSE, diagonalXYDuty); 
            return SUCCESS;
        } else if (x < 0) { // 左转
            L_T_Wheel.operateMotor(REVERSE, yDuty); 
            L_B_Wheel.operateMotor(REVERSE, yDuty); 
            R_T_Wheel.operateMotor(REVERSE, diagonalXYDuty); 
            R_B_Wheel.operateMotor(REVERSE, diagonalXYDuty); 
            return SUCCESS;
        } else if (x > 0) { // 右转
            L_T_Wheel.operateMotor(REVERSE, diagonalXYDuty); 
            L_B_Wheel.operateMotor(REVERSE, diagonalXYDuty); 
            R_T_Wheel.operateMotor(REVERSE, yDuty); 
            R_B_Wheel.operateMotor(REVERSE, yDuty); 
            return SUCCESS;   
        } 
    }

    return BEPENDING;
}
